#include "paceDetect.h"
struct pace_struct pace_det={
0,0,
	{0,0,0,},{0,0,0,0},
	0,0,
	0,
	{0,0,0,0,0},
	0,
	0,0,
	{0,0,0,0,0,0},
	0,
	0,
	0,0,
	0,0,0,0,
	0,0

};
const int PRE_PACE = 800;
#define TH_PACEdet_16KHz 70000//600000
int PreProcess( int data )
{
	int y_delay;//N-1?????
	int y1;     //?????zw?N?????
	int y_ma;   //y1??N?????
	int yt;     //???????

	pace_det.Sum1 = pace_det.Sum1  - pace_det.input_store[pace_det.index] + data;
	pace_det.input_store[pace_det.index] = data;
	y1 = pace_det.Sum1 >> 2;

	pace_det.Sum2 = pace_det.Sum2  - pace_det.aver1_store[pace_det.index] + y1;
	pace_det.aver1_store[pace_det.index] = y1;
	y_ma = pace_det.Sum2 >> 2;

	if ( ++pace_det.index >= 4 )
	{
		pace_det.index = 0;
	}
	y_delay = pace_det.input_store[pace_det.index];
	yt = y_delay - y_ma;
	return yt;
}
void SDFT( int data )
{
	//???????
	int temp;
	//real = data - ( ( x0[0] + x0[1] ) >> 1 );
        pace_det.real = ( 2*data+pace_det.x0[0]-pace_det.x0[1]-2*pace_det.x0[2] 
	                      -pace_det.x0[3]+pace_det.x0[4] )>>1;

	//temp = x0[0] - x0[1];
        temp = pace_det.x0[0] + pace_det.x0[1] - pace_det.x0[3] - pace_det.x0[4] ;
	pace_det.imag = (temp*886) + (temp>>1) + (temp>>2) + (temp>>5) + (temp>>6);
	pace_det.imag >>= 10;

	pace_det.x0[4] = pace_det.x0[3];
	pace_det.x0[3] = pace_det.x0[2];
	pace_det.x0[2] = pace_det.x0[1];
	pace_det.x0[1] = pace_det.x0[0];
	pace_det.x0[0] = data;
	return ;
}
long long Peak(long long datum)
{
	long long pk = 0;
	if( pace_det.timeSinceMax > 0 )
	{
		pace_det.timeSinceMax++;
	}
	if( (datum > pace_det.lastDatum) && (datum > pace_det.max) )
	{
		pace_det.max = datum ;
		if( pace_det.max > 2 )
		{
			pace_det.timeSinceMax = 1 ;
		}
	}

	else if( pace_det.timeSinceMax > 16 )
	{
		pk = pace_det.max ;
		pace_det.max = 0 ;
		pace_det.timeSinceMax = 0 ;
	}
	pace_det.lastDatum = datum ;
	return pk;	
}
char PACEdet()
{
	pace_det.Amp = ( (long long)(pace_det.real)*(long long)(pace_det.real)+
										(long long)(pace_det.imag)*(long long)(pace_det.imag) ) ;
	pace_det.Diff = pace_det.Amp - pace_det.Amp0 ;pace_det.Diff = ( pace_det.Diff >= 0 )? 
									pace_det.Diff : -pace_det.Diff ; 
	pace_det.Amp0 = pace_det.Amp ;
        
        long long output = 0;
        pace_det.Sum = pace_det.Sum  - pace_det.pInput[pace_det.index0] + pace_det.Diff;
        pace_det.pInput[pace_det.index0] = pace_det.Diff;
        if ( ++pace_det.index0 >= 6 )
	{
		pace_det.index0 = 0;
	}
        output = pace_det.Sum>>2;
        output = Peak(output) ;
        char flag = ( ( output > TH_PACEdet_16KHz ? 1 : 0 ) );
        if( pace_det.preBlankCnt >0 )
        {
          --pace_det.preBlankCnt;  
        }
        
        if( flag && !(pace_det.preBlankCnt) )
        {
            pace_det.preBlankCnt = PRE_PACE;
        }
        else
        {
            flag = 0;
        }
        return flag;
}
char PACEdet_16KHz( int data )
{
	char flag = 0 ;
	int yt = 0 ;
	yt = PreProcess( data );
	SDFT( yt );
	flag = PACEdet();
	return flag;
}